
PILINK_TYPE_TERMINAL   = 0x0004
PILINK_TYPE_CAM_CMD    = 0x0101
PILINK_TYPE_CAM_SENSOR_INFO = 0x0201
PILINK_TYPE_CAM_PARAMS = 0x0301
PILINK_TYPE_CAM_PARAM  = 0x0304   
PILINK_TYPE_UTC_TIME   = 0x0401

PILINK_TYPE_BOOT_CMD   = 0x4001
PILINK_TYPE_BOOT_DATA  = 0x4002
PILINK_TYPE_BOOT_ACK   = 0x4003

PILINK_TYPE_CAM_ACK    = 0xF101


PILINK_CAMERA_COMMAND_TAKE  = 0x0201
PILINK_CAMERA_COMMAND_FOCUS = 0x0204
PILINK_CAMERA_COMMAND_FLASH = 0x0208
PILINK_CAMERA_COMMAND_SET_PARAM  = 0x0210
PILINK_CAMERA_COMMAND_GET_PARAM  = 0x0220
PILINK_CAMERA_COMMAND_ACK_PARAM  = 0x0240
PILINK_CAMERA_COMMAND_SWITCH:int = 0x0301
PILINK_CAMERA_COMMAND_ENABLE_SHUTTER  = 0x0302
PILINK_CAMERA_COMMAND_DISABLE_SHUTTER = 0x0304
PILINK_CAMERA_COMMAND_REBOOT          = 0x0310

class PilinkCameraCmdStruct:
    def __init__(self):
        self.timestamp = 0
        self.command   = 0
        self.param1    = 0
        self.param2    = 0
        self.param3    = 0

PILINK_CAMERA_SENSOR_TYPE_TRIGGER     = 0x0201
PILINK_CAMERA_SENSOR_TYPE_TEMPERATURE = 0x0202
PILINK_CAMERA_SENSOR_TYPE_BRIGHT      = 0x0204
PILINK_CAMERA_SENSOR_TYPE_MOTION      = 0x0208

class PilinkCameraSensorInfo:
    def __init__(self):
        self.timestamp = 0
        self.type      = 0
        self.value1    = 0
        self.value2    = 0
        self.value3    = 0
        self.value4    = 0

class PilinkCameraUTCTime:
    def __init__(self):
        self.timestamp = 0
        self.tv_sec    = 0
        self.tv_msec   = 0
        self.year      = 0
        self.mon       = 0
        self.day       = 0
        self.hour      = 0
        self.min       = 0
        self.sec       = 0


PILINK_CAMERA_PARAM_TIMING_SHUTTER_ON  = 0x0101
PILINK_CAMERA_PARAM_TIMING_SHUTTER_OFF = 0x0102
PILINK_CAMERA_PARAM_TIMING_FLASH_ON    = 0x0103
PILINK_CAMERA_PARAM_TIMING_FLASH_OFF   = 0x0104
PILINK_CAMERA_PARAM_TIMING_TX_CAM_ON   = 0x0105
PILINK_CAMERA_PARAM_TIMING_FOCUS_ON    = 0x0106
PILINK_CAMERA_PARAM_TIMING_FOCUS_OFF   = 0x0107
PILINK_CAMERA_PARAM_TIMING_SHUTTER2_ON = 0x0108
PILINK_CAMERA_PARAM_TIMING_SHUTTER2_OFF = 0x0109


class PilinkCameraParams:
    def __init__(self):
        self.timestamp = 0
        self.param00   = 0
        self.param01   = 0
        self.param02   = 0
        self.param03   = 0
        self.param04   = 0
        self.param05   = 0
        self.param06   = 0
        self.param07   = 0
        self.param08   = 0
        self.param09   = 0
        self.param10   = 0
        self.param11   = 0


PILINK_CAMERA_PARAM_SET      = 0x0101,
PILINK_CAMERA_PARAM_GET      = 0x0104,
PILINK_CAMERA_PARAM_DEL      = 0x0108,
PILINK_CAMERA_PARAM_SAVE     = 0x0110,
PILINK_CAMERA_PARAM_LOAD     = 0x0120,
PILINK_CAMERA_PARAM_ACK      = 0x0140,
PILINK_CAMERA_PARAM_LIST     = 0x0180,

class PilinkCameraParam:
    def __init__(self):
        self.timestamp = 0
        self.cmd = 0
        self.name_len = 0
        self.name = bytearray(31)
        self.value = 0


class PilinkCameraAck:
    def __init__(self):
        self.timestamp = 0
        self.type   = 0
        self.param  = 0

class PilinkTerminalData:
    def __init__(self):
        self.timestamp = 0
        self.cmd = 0
        self.channel = 0
        self.msg_len = 0
        self.msg = bytearray(128)

PILINK_BOOT_CMD  = 0x0101 


PILINK_BOOT_CMD_ABORT     = 0x0001
PILINK_BOOT_CMD_RESTART   = 0x0002
PILINK_BOOT_CMD_BOOT      = 0x0003
PILINK_BOOT_CMD_ERASE     = 0x0004
PILINK_BOOT_CMD_STA_UGD   = 0x0005
PILINK_BOOT_CMD_END_UGD   = 0x0006


class PilinkBootCmd:
    def __init__(self):
        self.cmd = 0
        self.param0 = 0
        self.param1 = 0
        self.param2 = 0

class PilinkBootData:
    def __init__(self):
        self.addr = 0
        self.data = 0

class PilinkBootAck:
    def __init__(self):
        self.addr = 0
        self.data = 0

class PilinkStatus:
    GSTATUS_IDLE = 0
    GSTATUS_HEAD = 1
    GSTATUS_SENDER = 2
    GSTATUS_PORT1 = 3
    GSTATUS_PORT2 = 4
    GSTATUS_LEN1 = 5
    GSTATUS_LEN2 = 6
    GSTATUS_DATA = 7
    GSTATUS_CHECK = 8
    GSTATUS_END = 9
    GSTATUS_OK = 10

class PilinkHandler:
    PILINK_HEAD1 = 0xFA
    PILINK_HEAD2 = 0xAA

    PILINK_END1 = 0xFE
    PILINK_END2 = 0xFD

    def __init__(self, sender):
        self.status = PilinkStatus.GSTATUS_IDLE
        self.sender = sender
        self.port = 0
        self.rx_msg_len = 0
        self.rxbuf = bytearray(512)  # Ensure the buffer is large enough
        self.txbuf = bytearray(512)  # Ensure the buffer is large enough
        self.checksum = 0
        self.wptr = 0

    def reset(self):
        self.status = PilinkStatus.GSTATUS_IDLE

    def parse(self, ch):
        if self.status == PilinkStatus.GSTATUS_IDLE:
            if ch == self.PILINK_HEAD1:
                self.status = PilinkStatus.GSTATUS_HEAD
        elif self.status == PilinkStatus.GSTATUS_HEAD:
            if ch == self.PILINK_HEAD2:
                self.status = PilinkStatus.GSTATUS_SENDER
            else:
                self.status = PilinkStatus.GSTATUS_IDLE
        elif self.status == PilinkStatus.GSTATUS_SENDER:
            self.sender = ch
            self.status = PilinkStatus.GSTATUS_PORT1
        elif self.status == PilinkStatus.GSTATUS_PORT1:
            self.port = ch
            self.status = PilinkStatus.GSTATUS_PORT2
        elif self.status == PilinkStatus.GSTATUS_PORT2:
            self.port = self.port * 256 + ch
            self.status = PilinkStatus.GSTATUS_LEN1
        elif self.status == PilinkStatus.GSTATUS_LEN1:
            self.rx_msg_len = ch
            self.status = PilinkStatus.GSTATUS_LEN2
        elif self.status == PilinkStatus.GSTATUS_LEN2:
            self.rx_msg_len = self.rx_msg_len * 256 + ch
            self.status = PilinkStatus.GSTATUS_DATA
            self.wptr = 0
        elif self.status == PilinkStatus.GSTATUS_DATA:
            self.rxbuf[self.wptr] = ch
            self.wptr += 1
            if self.wptr > self.rx_msg_len - 1:
                self.status = PilinkStatus.GSTATUS_CHECK
            if self.wptr > 255:
                self.status = PilinkStatus.GSTATUS_IDLE
                return -1
        elif self.status == PilinkStatus.GSTATUS_CHECK:
            self.checksum = ch
            self.status = PilinkStatus.GSTATUS_END
        elif self.status == PilinkStatus.GSTATUS_END:
            if ch == self.PILINK_END1:
                self.status = PilinkStatus.GSTATUS_OK
            else:
                self.status = PilinkStatus.GSTATUS_IDLE
                return -2
        elif self.status == PilinkStatus.GSTATUS_OK:
            if ch == self.PILINK_END2:
                if self.checksum == self.calc_checksum():
                    self.status = PilinkStatus.GSTATUS_IDLE
                    return 1
                else:
                    self.status = PilinkStatus.GSTATUS_IDLE
                    print(f"cs:{self.checksum} vs {self.calc_checksum()}")
                    return -3
            else:
                self.status = PilinkStatus.GSTATUS_IDLE
                return -4
        else:
            self.status = PilinkStatus.GSTATUS_IDLE

        return 0

    def calc_checksum(self):
        # Corrected the range to avoid index out of bounds
        sum = 0
        for i in range(0, self.rx_msg_len):
            sum += self.rxbuf[i]
        return sum & 0xFF

    def get_data(self):
        # Added a check to ensure length does not exceed buf size
        return self.rxbuf[:min(self.rx_msg_len, len(self.rxbuf))]

    def msg_pack(self, port, dat, length):
        if length > len(self.txbuf) - 10:
            raise ValueError("buffer is too small to hold the message")

        counter = 0
        self.txbuf[counter] = PilinkHandler.PILINK_HEAD1
        counter += 1
        self.txbuf[counter] = PilinkHandler.PILINK_HEAD2
        counter += 1
        self.txbuf[counter] = self.sender
        counter += 1
        self.txbuf[counter] = port//256
        counter += 1
        self.txbuf[counter] = port%256
        counter += 1
        self.txbuf[counter] = length//256
        counter += 1
        self.txbuf[counter] = length%256
        counter += 1

        for i in range(length):
            self.txbuf[counter] = dat[i]
            counter += 1

        sum = 0
        for i in range(7, 7+length):  # Calculate checksum including the length
            sum += self.txbuf[i]
        self.txbuf[counter] = sum & 0xFF
        counter += 1
        self.txbuf[counter] = PilinkHandler.PILINK_END1
        counter += 1
        self.txbuf[counter] = PilinkHandler.PILINK_END2
        counter += 1

        return counter


def pilink_sensor_info_decode(buf):
    if len(buf) < 28:
        print("[pilink]buf for sensor info is too small")
        
    info = PilinkCameraSensorInfo()
    info.timestamp =int.from_bytes(buf[0:8], byteorder='little')
    info.type   =int.from_bytes(buf[8:12], byteorder='little')
    info.value1 =int.from_bytes(buf[12:16], byteorder='little')
    info.value2 =int.from_bytes(buf[16:20], byteorder='little')
    info.value3 =int.from_bytes(buf[20:24], byteorder='little')
    info.value4 =int.from_bytes(buf[24:28], byteorder='little')
    return info

def pilink_sensor_info_encode(info):
    ret = bytearray(28)
    buf = info.timestamp.to_bytes(8, byteorder='little')
    ret[0:8] = buf[0:8]
    buf = info.type.to_bytes(4, byteorder='little')
    ret[8:12] = buf[0:4]
    buf = info.value1.to_bytes(4, byteorder='little')
    ret[12:16] = buf[0:4]
    buf = info.value2.to_bytes(4, byteorder='little')
    ret[16:20] = buf[0:4]
    buf = info.value3.to_bytes(4, byteorder='little')
    ret[20:24] = buf[0:4]
    buf = info.value4.to_bytes(4, byteorder='little')
    ret[24:28] = buf[0:4]
    return ret


def pilink_camera_cmd_decode(bytearray):
    info = PilinkCameraCmdStruct()
    info.timestamp = int.from_bytes(bytearray[0:8], byteorder='little')
    info.command   = int.from_bytes(bytearray[8:12], byteorder='little')
    info.param1 = int.from_bytes(bytearray[12:16], byteorder='little')
    info.param2 = int.from_bytes(bytearray[16:20], byteorder='little')
    info.param3 = int.from_bytes(bytearray[20:24], byteorder='little')
    return info


def pilink_camera_cmd_encode(cmd):
    ret = bytearray(24)

    buf = cmd.timestamp.to_bytes(8, byteorder='little')
    ret[0:8] = buf[0:8]
    buf = cmd.command.to_bytes(4, byteorder='little')
    ret[8:12] = buf[0:4]
    buf = cmd.param1.to_bytes(4, byteorder='little')
    ret[12:16] = buf[0:4]
    buf = cmd.param2.to_bytes(4, byteorder='little')
    ret[16:20] = buf[0:4]
    buf = cmd.param3.to_bytes(4, byteorder='little')
    ret[20:24] = buf[0:4]
    return ret

def pilink_camera_UTC_time_decode(bytearray):
    info = PilinkCameraUTCTime()
    info.timestamp = int.from_bytes(bytearray[0:8], byteorder='little')
    info.tv_sec  = int.from_bytes(bytearray[8:12], byteorder='little')
    info.tv_msec = int.from_bytes(bytearray[12:16], byteorder='little')
    info.year    = int.from_bytes(bytearray[16:20], byteorder='little')
    info.mon     = int.from_bytes(bytearray[20:22], byteorder='little')
    info.day     = int.from_bytes(bytearray[22:24], byteorder='little')
    info.hour    = int.from_bytes(bytearray[24:26], byteorder='little')
    info.min     = int.from_bytes(bytearray[26:28], byteorder='little')
    info.sec     = int.from_bytes(bytearray[28:30], byteorder='little')

    return info


def pilink_camera_UTC_time_encode(cmd):
    ret = bytearray(30)
    buf = cmd.timestamp.to_bytes(8, byteorder='little')
    ret[0:8] = buf[0:8]
    buf = cmd.tv_sec.to_bytes(4, byteorder='little')
    ret[8:12] = buf[0:4]
    buf = cmd.tv_msec.to_bytes(4, byteorder='little')
    ret[12:16] = buf[0:4]
    buf = cmd.year.to_bytes(4, byteorder='little')
    ret[16:20] = buf[0:4]
    buf = cmd.mon.to_bytes(2, byteorder='little')
    ret[20:22] = buf[0:2]
    buf = cmd.day.to_bytes(2, byteorder='little')
    ret[22:24] = buf[0:2]
    buf = cmd.hour.to_bytes(2, byteorder='little')
    ret[24:26] = buf[0:2]
    buf = cmd.min.to_bytes(2, byteorder='little')
    ret[26:28] = buf[0:2]
    buf = cmd.sec.to_bytes(2, byteorder='little')
    ret[28:30] = buf[0:2]
    return ret


def pilink_camera_params_decode(bytearray):
    info = PilinkCameraParams()
    info.timestamp = int.from_bytes(bytearray[0:8], byteorder='little')
    info.param00 = int.from_bytes(bytearray[8:10], byteorder='little')
    info.param01 = int.from_bytes(bytearray[10:12], byteorder='little')
    info.param02 = int.from_bytes(bytearray[12:14], byteorder='little')
    info.param03 = int.from_bytes(bytearray[14:16], byteorder='little')
    info.param04 = int.from_bytes(bytearray[16:18], byteorder='little')
    info.param05 = int.from_bytes(bytearray[18:20], byteorder='little')
    info.param06 = int.from_bytes(bytearray[20:22], byteorder='little')
    info.param07 = int.from_bytes(bytearray[22:24], byteorder='little')
    info.param08 = int.from_bytes(bytearray[24:26], byteorder='little')
    info.param09 = int.from_bytes(bytearray[26:28], byteorder='little')
    info.param10 = int.from_bytes(bytearray[28:30], byteorder='little')
    info.param11 = int.from_bytes(bytearray[30:32], byteorder='little')

    return info


def pilink_camera_params_encode(cmd):
    ret = bytearray(56)
    buf = cmd.timestamp.to_bytes(8, byteorder='little')
    ret[0:8] = buf[0:8]
    buf = cmd.param00.to_bytes(2, byteorder='little')
    ret[8:10] = buf[0:2]
    buf = cmd.param01.to_bytes(2, byteorder='little')
    ret[10:12] = buf[0:2]
    buf = cmd.param02.to_bytes(2, byteorder='little')
    ret[12:14] = buf[0:2]
    buf = cmd.param03.to_bytes(2, byteorder='little')
    ret[14:16] = buf[0:2]
    buf = cmd.param04.to_bytes(2, byteorder='little')
    ret[16:18] = buf[0:2]
    buf = cmd.param05.to_bytes(2, byteorder='little')
    ret[18:20] = buf[0:2]
    buf = cmd.param06.to_bytes(2, byteorder='little')
    ret[20:22] = buf[0:2]
    buf = cmd.param07.to_bytes(2, byteorder='little')
    ret[22:24] = buf[0:2]
    buf = cmd.param08.to_bytes(2, byteorder='little')
    ret[24:26] = buf[0:2]
    buf = cmd.param09.to_bytes(2, byteorder='little')
    ret[26:28] = buf[0:2]
    buf = cmd.param10.to_bytes(2, byteorder='little')
    ret[28:30] = buf[0:2]
    buf = cmd.param11.to_bytes(2, byteorder='little')
    ret[30:32] = buf[0:2]
    
    return ret


def pilink_camera_param_decode(bytearray):
    param = PilinkCameraParam()

    param.timestamp = int.from_bytes(bytearray[0:8], byteorder='little')
    param.cmd       = int.from_bytes(bytearray[8:12], byteorder='little')
    param.name_len  = int(bytearray[12])
    if param.name_len > 31:
        param.name_len = 31
    param.name[:param.name_len] = bytearray[13:13+param.name_len]
    param.value     = int.from_bytes(bytearray[44:48], byteorder='little')

    return param

def pilink_camera_param_encode(param):
    ret = bytearray(48)
    buf = param.timestamp.to_bytes(8, byteorder='little')
    ret[0:8] = buf[0:8]
    buf = param.cmd.to_bytes(4, byteorder='little')
    ret[8:12] = buf[0:2]
    buf = param.name_len.to_bytes(1, byteorder='little')
    ret[12]   = buf[0]
    ret[13:44] = param.name[0:31]
    ret[43] = 0
    buf = param.value.to_bytes(4, byteorder='little')
    ret[44:48] = buf[0:4]
    
    return ret


def pilink_terminal_data_decode(buf):
    dat = PilinkTerminalData()

    if len(buf) >= 142:
        dat.timestamp = int.from_bytes(buf[0:8], byteorder='little')
        dat.cmd       = int.from_bytes(buf[8:10], byteorder='little')
        dat.channel   = int.from_bytes(buf[10:12], byteorder='little')
        dat.msg_len   = int.from_bytes(buf[12:14], byteorder='little')
        dat.msg[0:128]= buf[14:142]

    return dat

def pilink_terminal_data_encode(dat):
    ret = bytearray(142)
    buf = dat.timestamp.to_bytes(8, byteorder='little')
    ret[0:8] = buf[0:8]
    buf = dat.cmd.to_bytes(2, byteorder='little')
    ret[8:10] = buf[0:2]
    buf = dat.channel.to_bytes(2, byteorder='little')
    ret[10:12] = buf[0:2]
    buf = dat.msg_len.to_bytes(2, byteorder='little')
    ret[12:14] = buf[0:2]
    ret[14:142]= dat.msg[0:128]
    
    return ret


class pilink_terminal_parser:
    def __init__(self) -> None:
        pass
    
    def apply_msg(self, msg:PilinkTerminalData):
        msg.msg



def pilink_boot_cmd_decode(buf):
    dat = PilinkBootCmd()

    if len(buf) >= 28:
        dat.cmd      = int.from_bytes(buf[0:4], byteorder='little')
        dat.param0   = int.from_bytes(buf[4:8], byteorder='little')
        dat.param1   = int.from_bytes(buf[8:12], byteorder='little')
        dat.param2   = int.from_bytes(buf[12:16], byteorder='little')

    return dat

def pilink_boot_cmd_encode(dat):
    ret = bytearray(16)
    buf = dat.cmd.to_bytes(4, byteorder='little')
    ret[0:4] = buf[0:4]
    buf = dat.param0.to_bytes(4, byteorder='little')
    ret[4:8] = buf[0:4]
    buf = dat.param1.to_bytes(4, byteorder='little')
    ret[8:12] = buf[0:4]
    buf = dat.param2.to_bytes(4, byteorder='little')
    ret[12:16] = buf[0:4]

    return ret



def pilink_boot_data_decode(buf):
    dat = PilinkBootData()

    if len(buf) >= 9:
        dat.addr   = int.from_bytes(buf[0:4], byteorder='little')
        dat.data   = int.from_bytes(buf[4:8], byteorder='little')

    return dat

def pilink_boot_data_encode(dat):
    ret = bytearray(8)
    buf = dat.addr.to_bytes(4, byteorder='little')
    ret[0:4] = buf[0:4]
    buf = dat.data.to_bytes(4, byteorder='little')
    ret[4:8] = buf[0:4]

    return ret

def pilink_boot_ack_decode(buf):
    dat = PilinkBootData()

    if len(buf) >= 9:
        dat.addr   = int.from_bytes(buf[0:4], byteorder='little')
        dat.data   = int.from_bytes(buf[4:8], byteorder='little')

    return dat

def pilink_boot_ack_encode(dat):
    ret = bytearray(8)
    buf = dat.addr.to_bytes(4, byteorder='little')
    ret[0:4] = buf[0:4]
    buf = dat.data.to_bytes(4, byteorder='little')
    ret[4:8] = buf[0:4]

    return ret